/*!
 * @file EditObjectActuator.cpp
 * @author Rocco Martino
 */
/***************************************************************************
 *   Copyright (C) 2014 by Rocco Martino                                   *
 *   martinorocco@gmail.com                                                *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU Lesser General Public License as        *
 *   published by the Free Software Foundation; either version 2.1 of the  *
 *   License, or (at your option) any later version.                       *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this program; if not, write to the                 *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

/* ======================================================================= */
/* ....................................................................... */
#include <ooGame/EditObjectActuator>

#include <osgODE/Notify>
#include <osgODE/CommonWorldOperations>
#include <osgODE/RigidBody>
#include <osgODE/AutoRemoveUpdateCallback>
/* ....................................................................... */
/* ======================================================================= */




using namespace ooGame ;




/* ======================================================================= */
/* ....................................................................... */
EditObjectActuator::EditObjectActuator(Mode mode):
    m_mode                      ( mode ),
    m_life_time                 ( 0 ),
    m_linear_velocity_local     ( false ),
    m_angular_velocity_local    ( false ),
    m_dynamic_operation         ( SET_MASS ),
    m_mass                      ( 0.0 ),
    m_up_world                  ( osg::Z_AXIS ),
    m_up_local                  ( osg::Z_AXIS ),
    m_front_local               ( osg::Y_AXIS ),
    m_dirty                     ( true ),
    m_distance_min              ( 0.0 ),
    m_distance_max              ( FLT_MAX )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
EditObjectActuator::EditObjectActuator(const EditObjectActuator& other, const osg::CopyOp& copyop):
    Actuator                    ( other, copyop ),
    m_mode                      ( other.m_mode ),
    m_object                    ( other.m_object ),
    m_life_time                 ( other.m_life_time ),
    m_linear_velocity           ( other.m_linear_velocity ),
    m_angular_velocity          ( other.m_angular_velocity ),
    m_linear_velocity_local     ( other.m_linear_velocity_local ),
    m_angular_velocity_local    ( other.m_angular_velocity_local ),
    m_dynamic_operation         ( other.m_dynamic_operation ),
    m_mass                      ( other.m_mass ),
    m_up_world                  ( other.m_up_world ),
    m_up_local                  ( other.m_up_local ),
    m_front_local               ( other.m_front_local ),
    m_dirty                     ( other.m_dirty ),
    m_track_to_by_name_internal ( other.m_track_to_by_name_internal ),
    m_track_to                  ( other.m_track_to.get() ),
    m_linear_pid                ( osg::clone(other.m_linear_pid.get(), copyop) ),
    m_angular_pid               ( osg::clone(other.m_angular_pid.get(), copyop) ),
    m_distance_min              ( other.m_distance_min ),
    m_distance_max              ( other.m_distance_max )
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
EditObjectActuator::~EditObjectActuator(void)
{
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
EditObjectActuator::actuate( osgODE::RigidBody* body, Game* game )
{
    (void) game ;


    PS_ASSERT1( body ) ;
    PS_ASSERT1( body->getWorld() ) ;



    if( m_dirty ) {

        if( m_track_to_by_name_internal != "" ) {

            PS_DBG("ooGame::EditObjectActuator::actuate(%p, body=%p, game=%p): looking for \"%s\"", this, body, game, m_track_to_by_name_internal.c_str()) ;

            osgODE::ODEObject*  obj = body->getWorld()->getObjectByName( m_track_to_by_name_internal, true ) ;

            PS_ASSERT1( obj != NULL ) ;

            m_track_to = obj->asRigidBody() ;

            m_track_to_by_name_internal = "" ;

        }

        m_dirty = false ;
    }



    switch( m_mode )
    {
        case ADD_OBJECT:    addObject(body) ;   break ;
        case END_OBJECT:    endObject(body) ;   break ;
        case DYNAMICS:      dynamics(body) ;    break ;
        case TRACK_TO:      trackTo(body) ;     break ;

        default:            PS_BREAKPOINT() ;   break ;
    }
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
EditObjectActuator::addObject( osgODE::RigidBody* body )
{
    PS_ASSERT1( m_object.valid() ) ;

    if( ! m_object.valid() ) {
        PS_FATAL("ooGame::EditObjectActuator::addObject(%p): no object", this) ;
        return ;
    }


    osg::ref_ptr<osgODE::ODEObject> new_object = osg::clone( m_object.get(), osg::CopyOp::DEEP_COPY_CALLBACKS | osg::CopyOp::DEEP_COPY_USERDATA ) ;

    new_object->generateID() ;



    if( m_life_time != 0 ) {
        new_object->addUpdateCallback( new osgODE::AutoRemoveUpdateCallback(m_life_time) ) ;
    }




    osgODE::RigidBody*  new_body = new_object->asRigidBody() ;

    if( new_body ) {

        new_body->setQuaternion( body->getQuaternion() ) ;
        new_body->setPosition( body->getPosition() ) ;

        osg::Quat   quat = new_body->getQuaternion() ;

        osg::Vec3   lvel = m_linear_velocity_local ? quat * m_linear_velocity : m_linear_velocity ;
        osg::Vec3   avel = m_angular_velocity_local ? quat * m_angular_velocity : m_angular_velocity ;

        new_body->setLinearVelocity( lvel ) ;
        new_body->setAngularVelocity( avel ) ;
    }

    body->getWorld()->addOperation( new osgODE::AddObjectOperation(new_object) ) ;
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
EditObjectActuator::endObject( osgODE::RigidBody* body )
{
    body->getWorld()->addOperation( new osgODE::RemoveObjectOperation(body) ) ;
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
EditObjectActuator::dynamics( osgODE::RigidBody* body )
{
    switch( m_dynamic_operation )
    {
        case SET_MASS:              body->setMass( m_mass ) ;       break ;

        case DISABLE_RIGID_BODY:    body->setBodyEnabled( false ) ; break ;

        case ENABLE_RIGID_BODY:     body->setBodyEnabled( true ) ;  break ;

        case SET_KINEMATIC:         body->setKinematic( true ) ;    break ;

        case SET_DYNAMIC:           body->setKinematic( false ) ;   break ;

        default:                    PS_BREAKPOINT() ;               break ;
    }
}
/* ....................................................................... */
/* ======================================================================= */




/* ======================================================================= */
/* ....................................................................... */
void
EditObjectActuator::trackTo( osgODE::RigidBody* body )
{
    PS_ASSERT1( m_track_to.valid() ) ;


    //
    // angular
    //
    {
        osg::Quat   quat = body->getQuaternion() ;


        osg::Vec3   front = m_track_to->getPosition() - body->getPosition() ;

        ooReal  distance = front.normalize() ;




        // avoid assert in _dNormalize4

        const ooReal    small_number = 1.0e-5 ;

        if( distance > small_number ) {



            osg::Vec3   up = front ^ m_up_world ;

            up = up ^ front ;
            up.normalize() ;




            osg::Quat   rot1 ;

            rot1.makeRotate( quat * m_front_local, front ) ;



            osg::Quat   rot2 ;

            rot2.makeRotate( quat * m_up_local, up ) ;


            osg::Quat   qerr = rot1 * rot2 ;

            osg::Vec3   axis( qerr.x(), qerr.y(), qerr.z() ) ;

            float   angle = axis.normalize() ;

            if( osg::absolute( qerr.w() ) > 0.5 ) {
                angle = asin( angle ) * 2.0 ;
            } else {
                angle = acos( qerr.w() ) * 2.0 ;
            }

            if( angle == 0.0 ) {
                axis = osg::Z_AXIS ;
            }





            if( m_angular_pid.valid() ) {

                ooReal  mag = m_angular_pid->solve( angle, body->getWorld()->getCurrentStepSize() ) ;
                body->addTorque( axis * mag ) ;

            } else {
                unsigned int    K = m_life_time != 0 ? m_life_time : 1 ;

                body->addTorque( axis * angle * body->getMass() / (ooReal)K ) ;
            }
        }
    }
    //
    // end angular
    //






    //
    // linear
    //
    {
        osg::Vec3   direction = m_track_to->getPosition() - body->getPosition() ;
        ooReal      distance = direction.normalize() ;

        ooReal      err = 0 ;

        if( distance < m_distance_min ) {
            err = m_distance_min - distance ;
            direction *= -1.0 ;
        }

        else if( distance > m_distance_max ) {
            err = distance - m_distance_max ; ;
        }


        if( m_linear_pid.valid() ) {

            ooReal  mag = m_linear_pid->solve( err, body->getWorld()->getCurrentStepSize() ) ;
            body->addForce( direction * mag ) ;

        } else {
            unsigned int    K = m_life_time != 0 ? m_life_time : 1 ;

            body->addForce( direction * distance * body->getMass() / (ooReal)K ) ;
        }
    }
    //
    // end linear
    //
}
/* ....................................................................... */
/* ======================================================================= */
